﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.IO;

namespace Meta.Numerics.Matrices
{
    // we use the storage method described in Knuth
    // at five values per entry, this is not the most storage-efficient format
    // but it is conceptually straightforward and allows easy update
    // other formats i have seen, e.g. Boeing-Harwell, require that the matrix entries be supplied in a particular, non-random order
    // and do not allow easy update

    internal class SymmetricSparseMatrixElement
    {
        public SymmetricSparseMatrixElement(int row, int column, double value)
        {
            Row = row;
            Column = column;
            Value = value;
        }

        public int Row;
        public int Column;
        public double Value;
        public SymmetricSparseMatrixElement NextInRow;
        public SymmetricSparseMatrixElement NextInColumn;
    }

    public sealed class SymmetricSparseMatrix : AnySquareMatrix
    {
        private int dimension;
        private SymmetricSparseMatrixElement[] rows;
        private SymmetricSparseMatrixElement[] columns;
        private int fill;

        /// <summary>
        /// Initializes a new sparse, square matrix.
        /// </summary>
        /// <param name="dimension">The dimension of the matrix, which must be positive.</param>
        public SymmetricSparseMatrix(int dimension)
        {
            if (dimension < 1) throw new ArgumentOutOfRangeException("dimension");
            this.dimension = dimension;
            rows = new SymmetricSparseMatrixElement[dimension];
            columns = new SymmetricSparseMatrixElement[dimension];
            fill = 0;
        }

        internal SymmetricSparseMatrix(int dimension, SymmetricSparseMatrixElement[] rows, SymmetricSparseMatrixElement[] columns, int fill)
        {
            this.dimension = dimension;
            this.rows = rows;
            this.columns = columns;
            this.fill = fill;
        }

        /// <inheritdoc />
        public override int Dimension
        {
            get
            {
                return (dimension);
            }
        }

        /// <inheritdoc />
        public override double this[int r, int c]
        {
            get
            {
                if ((r < 0) || (r >= dimension)) throw new ArgumentOutOfRangeException("r");
                if ((c < 0) || (c >= dimension)) throw new ArgumentOutOfRangeException("c");
                SymmetricSparseMatrixElement element;
                if (c < r)
                {
                    element = GetElement(r, c);
                }
                else
                {
                    element = GetElement(c, r);
                }
                if (element == null)
                {
                    return (0.0);
                }
                else
                {
                    return (element.Value);
                }
            }
            set
            {
                if ((r < 0) || (r >= dimension)) throw new ArgumentOutOfRangeException("r");
                if ((c < 0) || (c >= dimension)) throw new ArgumentOutOfRangeException("c");
                if (c < r)
                {
                    SetElement(r, c, value);
                }
                else
                {
                    SetElement(c, r, value);
                }

            }
        }

        private SymmetricSparseMatrixElement GetElement(int r, int c)
        {
            SymmetricSparseMatrixElement current = rows[r];
            while (current != null)
            {
                if (current.Column == c) return (current);
                current = current.NextInRow;
            }
            return (null);
        }

        private SymmetricSparseMatrixElement SetElement(int r, int c, double value)
        {

            // if the row is empty: previousInRow == null, nextInRow == null
            // if the new element is the leftmost in the row: previousInRow == null, nextInRow != null
            // if the new element is intermediate in the row: previousInRow != null, nextInRow != null
            // if the new element is the rightmost in the row: previousInRow != null, nextInRow == null
            // if the element already exists: nextInRow is the element element, previousInRow is null if it is the first in the row
            SymmetricSparseMatrixElement previousInRow = null;
            SymmetricSparseMatrixElement nextInRow = rows[r];
            while ((nextInRow != null) && (nextInRow.Column < c))
            {
                previousInRow = nextInRow;
                nextInRow = nextInRow.NextInRow;
            }

            SymmetricSparseMatrixElement previousInColumn = null;
            SymmetricSparseMatrixElement nextInColumn = columns[c];
            while ((nextInColumn != null) && (nextInColumn.Row < r))
            {
                previousInColumn = nextInColumn;
                nextInColumn = nextInColumn.NextInColumn;
            }

            if ((nextInRow != null) && (nextInRow.Row == r) && (nextInRow.Column == c))
            {
                // the element already exists
                if (value != 0.0)
                {
                    // if the value is non-zero, just update it
                    nextInRow.Value = value;
                    return (nextInRow);
                }
                else
                {
                    // if the value is zero, delete the element
                    if (previousInRow == null)
                    {
                        rows[r] = nextInRow.NextInRow;
                    }
                    else
                    {
                        previousInRow.NextInRow = nextInRow.NextInRow;
                    }
                    if (previousInColumn == null)
                    {
                        columns[c] = nextInColumn.NextInColumn;
                    }
                    else
                    {
                        previousInColumn.NextInColumn = nextInColumn.NextInColumn;
                    }
                    fill--;
                    return (null);
                }
            }
            else
            {
                // the element does not yet exist
                if (value == 0.0)
                {
                    // if the value is zero, we don't need to do anything
                    return (null);
                }
                else
                {
                    // otherwise, we need to create an element
                    SymmetricSparseMatrixElement element = new SymmetricSparseMatrixElement(r, c, value);
                    if (previousInRow == null)
                    {
                        rows[r] = element;
                    }
                    else
                    {
                        previousInRow.NextInRow = element;
                    }
                    element.NextInRow = nextInRow;
                    if (previousInColumn == null)
                    {
                        columns[c] = element;
                    }
                    else
                    {
                        previousInColumn.NextInColumn = element;
                    }
                    element.NextInColumn = nextInColumn;
                    fill++;
                    return (element);
                }
            }

        }

        public RealEigensystem Eigensystem()
        {

            //double[][] A = SymmetricMatrixAlgorithms.Copy(values, dimension);
            SymmetricSparseMatrix A = this;
            double[] V = SquareMatrixAlgorithms.CreateUnitMatrix(dimension);
            SymmetricSparseMatrixAlgorithms.JacobiEigensystem(A, V, dimension);
            return (new RealEigensystem(dimension, SymmetricSparseMatrixAlgorithms.GetDiagonal(A, dimension), V));
        }

        /// <summary>
        /// Computes the eigenvalues of the matrix. 
        /// </summary>
        /// <returns>An array containing the matrix eigenvalues.</returns>
        /// <remarks>
        /// <para>If you require only the eigenvalues of the matrix, not its eigenvectors, this method will return them faster than
        /// the <see cref="Eigensystem"/> method. If you do need the eigenvectors as well as the eigenvalues, use the <see cref="Eigensystem"/>
        /// method instead.</para>
        /// </remarks>
        /*public double[] Eigenvalues () {

            double[][] A = SymmetricMatrixAlgorithms.Copy(values, dimension);
            SymmetricMatrixAlgorithms.JacobiEigensystem(A, null, dimension);
            return (SymmetricMatrixAlgorithms.GetDiagonal(A, dimension));

        }*/
        
    }
    internal static class SymmetricSparseMatrixAlgorithms {


        /*public static double[][] Copy (double[][] A, int dimension) {
            double[][] B = new double[dimension][];
            for (int i = 0; i < dimension; i++) {
                B[i] = new double[i + 1];
                Array.Copy(A[i], B[i], i + 1);
            }
            return (B);
        }
        */
        public static double[] GetDiagonal(SymmetricSparseMatrix A, int dimension)
        {
            double[] D = new double[dimension];
            for (int i = 0; i < dimension; i++) {
                D[i] = A[i,i];
            }
            return (D);
        }
        
   
        public static void JacobiEigensystem (SymmetricSparseMatrix A, double[] V, int dimension) {

            int count = 0;
            double sum;

            do {

                // check for non-convergence
                count++;
                if (count > 50) throw new NonconvergenceException();

                // sweep over off-diagonal elements p,q where q < p
                for (int p = 0; p < dimension; p++) {
                    for (int q = 0; q < p; q++) {

                        //Console.WriteLine("n={0}, p={1}, q={2}", count, p, q);

                        // skip if already zero
                        double M_pq = A[p,q];
                        if (M_pq == 0.0) continue;

                        // compute the rotation
                        // -pi/4 < phi < pi/4 is the rotation 
                        // s, c, and t are its sine, cosine, and tangent
                        // theta = cot(2 phi) and t2 = tan(phi/2)
                        double theta = 0.5 * (A[q,q] - A[p,p]) / M_pq;
                        double t;
                        if (theta > 0) {
                            t = 1.0 / (Math.Sqrt(theta * theta + 1.0) + theta);
                        } else {
                            t = -1.0 / (Math.Sqrt(theta * theta + 1.0) - theta);
                        }
                        double c = 1.0 / Math.Sqrt(1.0 + t * t);
                        double s = t * c;
                        double t2 = s / (1.0 + c);

                        // do the rotation
                        A[p,p] += -t * M_pq;
                        A[q,q] += t * M_pq;
                        for (int r = 0; r < q; r++) {
                            double M_pr = A[p,r];
                            double M_qr = A[q,r];
                            A[p,r] = M_pr - s * (M_qr + t2 * M_pr);
                            A[q,r] = M_qr + s * (M_pr - t2 * M_qr);
                        }
                        for (int r = q + 1; r < p; r++) {
                            double M_pr = A[p,r];
                            double M_rq = A[r,q];
                            A[p,r] = M_pr - s * (M_rq + t2 * M_pr);
                            A[r,q] = M_rq + s * (M_pr - t2 * M_rq);
                        }
                        for (int r = p + 1; r < dimension; r++) {
                            double M_rp = A[r,p];
                            double M_rq = A[r,q];
                            A[r,p] = M_rp - s * (M_rq + t2 * M_rp);
                            A[r,q] = M_rq + s * (M_rp - t2 * M_rq);
                        }
                        A[p,q] = 0.0;

                        // accumulate the rotations, if we are keeping track of them
                        if (V != null) {
                            // this update should be in BLAS, because it addresses underlying structure,
                            // but doing it using SetEntry and GetEntry is about 3x slower
                            int ip = dimension * p;
                            int iq = dimension * q;
                            for (int r = 0; r < dimension; r++) {
                                double V_rp = V[ip];
                                double V_rq = V[iq];
                                V[ip] = c * V_rp - s * V_rq;
                                V[iq] = s * V_rp + c * V_rq;
                                ip++;
                                iq++;
                                //double V_rp = MatrixAlgorithms.GetEntry(V, dimension, dimension, r, p);
                                //double V_rq = MatrixAlgorithms.GetEntry(V, dimension, dimension, r, q);
                                //MatrixAlgorithms.SetEntry(V, dimension, dimension, r, p, c * V_rp - s * V_rq);
                                //MatrixAlgorithms.SetEntry(V, dimension, dimension, r, q, s * V_rp + c * V_rq);
                            }
                        }

                    }
                }

                // sum off-diagonal elements
                sum = 0.0;
                for (int p = 0; p < dimension; p++) {
                    for (int q = 0; q < p; q++) {
                        double M_pq = A[p,q];
                        if (M_pq > 0) {
                            sum += M_pq;
                        } else {
                            sum -= M_pq;
                        }
                    }
                }

            } while (sum > 0.0);

        }

    }
}